package org.scalpel.mainApps.HHObjects;

import java.text.DecimalFormat;
import java.text.NumberFormat;

import org.opencv.core.Core;
import org.opencv.core.Point;
//import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.Scalar;

import org.scalpel.HHObjects.MapManager;
import org.scalpel.common.HHObject;
import org.scalpel.common.KeyPoint;

import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;

import ioio.lib.api.AnalogInput;
import ioio.lib.api.IOIO;
import ioio.lib.api.exception.ConnectionLostException;

/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * GenericVirtualMotor : This class allow differential drive motor simulation.
 **/
/* Data from the sharpGP2D12 datasheet
mm  V     1/V
004 1.370 0.730
010 1.844 0.542
020 2.222 0.450
026 2.732 0.366
030 3.022 0.331
036 2.970 0.337
040 2.722 0.367
050 2.332 0.429
060 2.010 0.498
070 1.754 0.570
080 1.554 0.644
090 1.400 0.714
100 1.254 0.797
120 1.046 0.956
140 0.932 1.073
160 0.822 1.217
180 0.733 1.364
200 0.655 1.527
250 0.533 1.876
300 0.433 2.309
350 0.377 2.653
400 0.311 3.215
*/

public class SharpGP2D12 extends HHObject {

	private final static float [][] fLut = new float[][]{
		/* 1/V	 meter */
		new float[]{0.730f, 0.004f}, /*  0 */
		new float[]{0.542f, 0.010f}, /*  1 */
		new float[]{0.450f, 0.020f}, /*  2 */
		new float[]{0.366f, 0.026f}, /*  3 */
		new float[]{0.331f, 0.030f}, /*  4 */
		new float[]{0.337f, 0.036f}, /*  5 */
		new float[]{0.367f, 0.040f}, /*  6 */
		new float[]{0.429f, 0.050f}, /*  7 */
		new float[]{0.498f, 0.060f}, /*  8 */
		new float[]{0.570f, 0.070f}, /*  9 */
		new float[]{0.644f, 0.080f}, /* 10 */
		new float[]{0.714f, 0.090f}, /* 11 */
		new float[]{0.797f, 0.100f}, /* 12 */
		new float[]{0.956f, 0.120f}, /* 13 */
		new float[]{1.073f, 0.140f}, /* 14 */
		new float[]{1.217f, 0.160f}, /* 15 */
		new float[]{1.364f, 0.180f}, /* 16 */
		new float[]{1.527f, 0.200f}, /* 17 */
		new float[]{1.876f, 0.250f}, /* 18 */
		new float[]{2.309f, 0.300f}, /* 19 */
		new float[]{2.653f, 0.350f}, /* 20 */
		new float[]{3.215f, 0.400f}  /* 21 */
	};

	
  private double fDistanceDetection = 0.4;
  private boolean fIsSimulation = false;
	private int fPin = 0;
	private IOIO fIoio = null;
	private KeyPoint fPosition = new KeyPoint();
		
	private AnalogInput fAnalogInput = null;

	private double fMeterToPix;
  private KeyPoint fCurrPos;
  
  //private Mat fArr = null;
  
  NumberFormat fNumberFormat;
  
  float fVoltage = 0.f;
    
	public SharpGP2D12(String name, int pin, double px, double py, double pa, double meterToPix){
		super(name);
		
	  fPin = pin;
	  fMeterToPix = meterToPix;
	  fPosition.x = px;
	  fPosition.y = py;
	  fPosition.a = pa;
	  	  
	  //fArr = new Mat((int)(meterToPix * fLut[21][1] * 1.5 + 1), 1, CvType.CV_8U);
	  
	  fNumberFormat = new DecimalFormat("0.000");
	  
	}

	protected void finalize() throws Throwable
	{
		if( fAnalogInput != null)
			fAnalogInput.close();
	  super.finalize(); //not necessary if extending Object.
	} 
	
	public void initIOIO(boolean isSimulation, IOIO ioio){
		fIsSimulation = isSimulation;

		if(fIsSimulation) return;
		
		fIoio = ioio;
		try {
			fAnalogInput = fIoio.openAnalogInput(fPin);
		} catch (ConnectionLostException e) {
			e.printStackTrace();
		};
		
	}
	
	public void updateIOIO(){
		if( fIsSimulation ) return;
		
		if( fIoio == null || 
				fAnalogInput == null ||
				fCurrPos == null
				) return;
		
		fVoltage =  1.f/fLut[21][0];
		try {
			fVoltage = fAnalogInput.getVoltage();
		} catch (ConnectionLostException e) {
			e.printStackTrace();
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		synchronized(this) {
			fDistanceDetection = convertVolt2Meter(fVoltage);
		}
	}

	public void setCurrentPosition(KeyPoint currPos)
	{		
		fCurrPos = currPos;
	}

	public void onDraw(Canvas canvas, Paint paint){
		if( fCurrPos == null) return;

		
		KeyPoint p = new KeyPoint();
		p.x = fPosition.x;
		p.y = fPosition.y;
		p.a = fPosition.a;
		Point p1 = MapManager.rotationPoint(
				fPosition.x + fCurrPos.x, 
				fPosition.y + fCurrPos.y, 
				fCurrPos, fCurrPos.a, fMeterToPix);
		
		synchronized(this) {
			p.x += fDistanceDetection;
		}
		p.rotate(fPosition, fPosition.a);
		Point p2 = MapManager.rotationPoint(
				p.x + fCurrPos.x, 
				p.y + fCurrPos.y, 
				fCurrPos, fCurrPos.a, fMeterToPix);
		
		
		paint.setColor(Color.RED);
		canvas.drawLine((float)p1.x, (float)p1.y, (float)p2.x, (float)p2.y, paint);
		/*
		paint.setColor(Color.BLACK);
		String posString = fNumberFormat.format(fDistanceDetection);
  	canvas.drawText(posString, p2.x(), p2.y(), paint);
  	posString = fNumberFormat.format(fVoltage);
  	canvas.drawText(posString, p2.x(), p2.y()+10, paint);
		 */

	}

	synchronized public double getDistanceDetected(){
		return fDistanceDetection;
	}

	public void drawNoObstacleLine(Canvas canvas, Paint paint, Mat probaImg, int zoomFactor){
		if( fCurrPos == null) return;

		//boolean foundSomething = false;
		KeyPoint p = new KeyPoint();
		p.x = fPosition.x;
		p.y = fPosition.y;
		p.a = fPosition.a;
		Point p1 = MapManager.rotationPoint(
				fPosition.x + fCurrPos.x, 
				fPosition.y + fCurrPos.y, 
				fCurrPos, fCurrPos.a, fMeterToPix/zoomFactor);
		
		synchronized(this) {
			p.x += fDistanceDetection;
		};
		p.rotate(fPosition, fPosition.a);
		Point p2 = MapManager.rotationPoint(
				p.x + fCurrPos.x, 
				p.y + fCurrPos.y, 
				fCurrPos, fCurrPos.a, fMeterToPix/zoomFactor);
		
		
		if(fIsSimulation){
			/*
			double distDetect = fLut[21][1];

			Point p1bis = MapManager.rotationPoint(
					fPosition.x + fCurrPos.x, 
					fPosition.y + fCurrPos.y, 
					fCurrPos, fCurrPos.a, fMeterToPix/zoomFactor);
			

			p.x = fPosition.x + distDetect;
			p.y = fPosition.y;
			p.a = fPosition.a;
			
			p.rotate(fPosition, fPosition.a);
			Point p2bis = MapManager.rotationPoint(
					p.x + fCurrPos.x, 
					p.y + fCurrPos.y, 
					fCurrPos, fCurrPos.a, fMeterToPix/zoomFactor);


			int x = (int) p1bis.x;
			int y = (int) p1bis.y;
			if( x < 0 ) x = 0;
			if( y < 0 ) y = 0;
			if( x > probaImg.width() ) x = probaImg.width();
			if( y > probaImg.height() ) y = probaImg.height();
			p1bis.set(new double[]{x, y});
			
			x = (int) p2bis.x;
			y = (int) p2bis.y;
			if( x < 0 ) x = 0;
			if( y < 0 ) y = 0;
			if( x > probaImg.width() ) x = probaImg.width();
			if( y > probaImg.height() ) y = probaImg.height();
			p2bis.set(new double[]{x, y});
			
			BytePointer iii = fArr.dataAddr();//data_ptr();
			Imgproc.();
			int lineNb = Core.line(probaImg, p1bis, p2bis, color, 8);//.cvSampleLine(probaImg, p1bis, p2bis, iii, 8);


			int val = 0;
		  for(int i = 0; i < lineNb; i++)
		  {
		  	Byte r = iii.get(i);
		  	val = r;
		  	if( r < 0){ // unwrap byte value
		  		val = 256 + r;
		  	}
		  	if( val >= 200 ){
		  		distDetect = (double)((i-2) * zoomFactor) / fMeterToPix;
		  		p2bis.set( new double[]{p1bis.x + (p2bis.x - p1bis.x) * (i-2) / lineNb,
		  						p1bis.y + (p2bis.y - p1bis.y) * (i-2) / lineNb});
		  		break;
		  	}
		  }
		  p2.set(new double[]{p2bis.x, p2bis.y});
			synchronized(this) {
				fDistanceDetection = distDetect;
			}*/
		}

	  
		Core.line(probaImg, p1, p2, new Scalar(0),//CvScalar.BLACK
								1, 
								16,//opencv_core.CV_AA
								0);
		if( !fIsSimulation && fDistanceDetection < fLut[fLut.length-1][1]){
			// TODO see if cvSetAt could be the solution
			probaImg.put((int)p2.y, (int)p2.x, 255.0/*WHITE*/);
		} 
		 
	}
	
	private double convertVolt2Meter(double valueInVolt)
	{	
		
		float vinv = (float) (1.f / valueInVolt);
		int i = fLut.length-1;
		float distance = fLut[i][1];
		while( i > 0){
			if( vinv <= fLut[i][0] && vinv > fLut[i-1][0]){
				float fact = 1.f - (fLut[i][0] - vinv) / (fLut[i][0] - fLut[i-1][0]);
				distance = fLut[i-1][1] + fact * (fLut[i][1] - fLut[i-1][1]);
			}
			i--;
		}
		return distance;
	}

	@Override
	public void finish() {
		// TODO Auto-generated method stub
		
	}
	
}
